import socket
import time
import sys
import os

class Go2wController:
    def __init__(self, ip, port):
        self.ip = ip
        self.port = port
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.sock.connect((ip, port))

    def send_cmd_vel(self, x, y, w):
        data_part = f"CMD,{x},{y},{w}"
        cs = 0
        for c in data_part:
            cs ^= ord(c)
        packet = f"${data_part}*{cs:02X}\n"
        self.sock.sendall(packet.encode("utf-8"))

    def close(self):
        self.sock.close()


# 平台相关键盘输入类
if os.name == 'nt':  # Windows
    import msvcrt

    class KeyPoller:
        def __enter__(self):
            return self

        def __exit__(self, type, value, traceback):
            pass

        def poll(self):
            if msvcrt.kbhit():
                return msvcrt.getch().decode('utf-8', errors='ignore')
            return None

else:  # Linux/macOS
    import tty
    import termios
    import select

    class KeyPoller:
        def __enter__(self):
            self.fd = sys.stdin.fileno()
            self.new = termios.tcgetattr(self.fd)
            new_attr = termios.tcgetattr(self.fd)
            new_attr[3] &= ~termios.ICANON & ~termios.ECHO
            termios.tcsetattr(self.fd, termios.TCSAFLUSH, new_attr)
            return self

        def __exit__(self, type, value, traceback):
            termios.tcsetattr(self.fd, termios.TCSAFLUSH, self.new)

        def poll(self):
            if select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []):
                return sys.stdin.read(1)
            return None


if __name__ == "__main__":
    ip = "192.168.123.18"
    port = 6002
    controller = Go2wController(ip, port)

    try:
        with KeyPoller() as key_poller:
            pressed_keys = set()

            while True:
                key = key_poller.poll()
                if key:
                    if key in ['w', 's', 'a', 'd', 'q', 'e']:
                        pressed_keys.add(key)
                    elif key == 'q':
                        break
                else:
                    # 模拟按键释放（无新输入时清空键集）
                    pressed_keys.clear()

                # 根据当前按下的键计算速度
                x = 0.0
                y = 0.0
                w = 0.0

                if 'w' in pressed_keys:
                    x += 1.0
                if 's' in pressed_keys:
                    x -= 1.0
                if 'a' in pressed_keys:
                    y += 1.0
                if 'd' in pressed_keys:
                    y -= 1.0
                if 'q' in pressed_keys:
                    w += 1.0
                if 'e' in pressed_keys:
                    w -= 1.0

                controller.send_cmd_vel(x, y, w)
                time.sleep(0.05)

    except KeyboardInterrupt:
        pass
    finally:
        controller.close()